// v1.0 05/04/13 created
// v2.0 05/04/13 added some changes in callSIFTMatch

#include "ObjectDetector.hpp"

using namespace std;
using namespace cv;



void XtractMatchingPoints(const vector<KeyPoint>& keypoints_obj, const vector<KeyPoint>& keypoints_img, const vector<DMatch>& matches,
						  Mat& matched_pnts_obj, Mat& matched_pnts_img)
{

	// extract matching points
	matched_pnts_obj = Mat::zeros(1, matches.size(), CV_32FC2);
	matched_pnts_img = Mat::zeros(1, matches.size(), CV_32FC2);

	DMatch match;
	//for (int i = 0; i < matches.size(); i++)
	//for (int i = 0; i < 4; i++)
	for (int i = 0; i < matches.size(); i++)
	{
		match = matches[i];
		//cout << match.queryIdx <<  " -> " << match.trainIdx << endl;
		matched_pnts_obj.at<Vec2f>(i) = keypoints_obj[match.queryIdx].pt;                // works OK!!!!
		matched_pnts_img.at<Vec2f>(i) = keypoints_img[match.trainIdx].pt;                

		float dist = match.distance;

		//printf("(%0.2f, %0.2f) -> (%0.2f, %0.2f) and dist = %0.2f\n",matched_pnts_obj.at<Vec2f>(i)[0],matched_pnts_obj.at<Vec2f>(i)[1], matched_pnts_img.at<Vec2f>(i)[0], matched_pnts_img.at<Vec2f>(i)[1], dist);
	}
}

// Date Created: 14/09/14
// Descriptions: original modification from OpenCV sample
// Status:  - works OK!!
//			- match drawing is good!
int callSIFTMatch(const Mat ref_img, const Mat frame)
{
	const int sample_sz = 400;

	Mat img;    // img  : reference image
	Mat img1;   // img1 : current image

	ref_img.copyTo(img);    // ref image
	frame.copyTo(img1);   // current image

	char *org_wind = "the box";
	char *scene_wind = "box in scene";

	
	//SIFT SIFTdect_obj = SIFT(sample_sz, 3, 0.04, 10, 1.6);
	SIFT SIFTdect_obj = SIFT(sample_sz, 3, 0.05, 10, 1.6);
	
	//SIFT SIFTdect_img = SIFT(sample_sz, 3, 0.04, 10, 1.6);
	SIFT SIFTdect_img = SIFT(sample_sz, 3, 0.05, 10, 1.6);

	vector<KeyPoint> keypoints_obj, keypoints_img;
	Mat descriptors_obj, descriptors_img; 

	
	SIFTdect_obj(img, noArray(), keypoints_obj, descriptors_obj, false);  // noArray(): using no mask
	SIFTdect_img(img1, noArray(), keypoints_img, descriptors_img, false);

	cout << "object =" << endl;
	MatInfo(img);
	cout << "scene =" << endl;
	MatInfo(img1);
	MatInfo(descriptors_obj);
	//getchar();
	//MATmtxdisp(descriptors_obj);
	//getchar();
	
	MatInfo(descriptors_img);
	//getchar();
	//MATmtxdisp(descriptors_img);
	//getchar();
	

	Mat pnts_obj = Mat::zeros(1, keypoints_obj.size(), CV_32FC2);
	Mat pnts_img = Mat::zeros(1, keypoints_img.size(), CV_32FC2);
	
	for (int i = 0; i < keypoints_obj.size() ; i++)
	{
		//printf("(%0.2f, %0.2f)\n", keypoints_obj[i].pt.x, keypoints_obj[i].pt.y);
		pnts_obj.at<Vec2f>(i)[0] = keypoints_obj[i].pt.x;
		pnts_obj.at<Vec2f>(i)[1] = keypoints_obj[i].pt.y;
	}
	
	for (int i = 0; i < keypoints_img.size() ; i++)
	{
		//printf("(%0.2f, %0.2f)\n", keypoints_img[i].pt.x, keypoints_img[i].pt.y);
		pnts_img.at<Vec2f>(i)[0] = keypoints_img[i].pt.x;
		pnts_img.at<Vec2f>(i)[1] = keypoints_img[i].pt.y;
	}


	drawCircles(img, pnts_obj);
	drawCircles(img1, pnts_img);


	// Match keypoints with Brute Force Matcher
	BFMatcher kpMatcher(NORM_L2, false);

	vector<DMatch> matches;

	Mat mask = Mat::zeros(descriptors_obj.rows, descriptors_img.rows, CV_8UC1);
	//mask.at<float>(keypoints_obj[0].pt.y, keypoints_obj[0].pt.x) = descriptors_obj.at<float>(keypoints_obj[0].pt.y, keypoints_obj[0].pt.x);
	if (sample_sz <= descriptors_obj.rows) 
	{
		Mat maskinit = Mat::ones(sample_sz, descriptors_img.rows, CV_8UC1);
		maskinit.copyTo(mask(Range(0, sample_sz), Range(0, descriptors_img.rows)));
	} else
	{
		Mat maskinit = Mat::ones(descriptors_obj.rows, descriptors_img.rows, CV_8UC1);
		maskinit.copyTo(mask(Range(0, descriptors_obj.rows), Range(0, descriptors_img.rows)));
	}

	kpMatcher.match(descriptors_obj, descriptors_img, matches, mask);   // no use mask -> skip the arg

		
	cout << "Number of matches = " << matches.size() << endl;

	//imshow(org_wind, img);
	//imshow(scene_wind, img1);
	
	
	// Using FUNDAMENTAL matrix
	// read matches
	/*
	DMatch match = matches[0];
	cout << match.queryIdx << " matches with " << match.imgIdx << " img idx " << match.imgIdx << endl;
	
	for (int i = 0; i < matches.size(); i++)
	{
		match = matches[i];
		cout << match.queryIdx <<  " -> " << match.trainIdx << endl;
	}
	*/
	
	double max_dist = 0; double min_dist = 500;

	//-- Quick calculation of max and min distances between keypoints
	for( int i = 0; i < matches.size(); i++ )
	{	DMatch match = matches[i];
		float dist = match.distance;
		if( dist < min_dist ) min_dist = dist;
		if( dist > max_dist ) max_dist = dist;
	}
	
	cout << "max_dist = " << max_dist << endl;
	cout << "min_dist = " << min_dist << endl;

	// find the good matches
	
	//-- Draw only "good" matches (i.e. whose distance is less than 3*min_dist )
	std::vector< DMatch > good_matches;

	for( int i = 0; i < matches.size(); i++ )
	{
		//if( matches[i].distance < 30*min_dist )   // change 3/4/5/6
		{ good_matches.push_back( matches[i]); }    // get all matches
	}
	

	// Show results of matching (good matches)
	Mat outimg;
	drawMatches(img, keypoints_obj, img1, keypoints_img, good_matches, outimg);
	imshow("Match result", outimg);


	// call XtractMachingPoints
	Mat matched_pnts_obj, matched_pnts_img;
	XtractMatchingPoints(keypoints_obj, keypoints_img, good_matches, matched_pnts_obj, matched_pnts_img);
	
	//Mat funda = findFundamentalMat(matched_pnts_obj, matched_pnts_img, noArray(), CV_FM_8POINT);  -> can't work
	//Mat funda = findFundamentalMat(matched_pnts_obj, matched_pnts_img, noArray, FM_RANSAC, 3, 0.8); -> can't work
	//Mat funda =	findFundamentalMat(matched_pnts_obj, matched_pnts_img, FM_RANSAC, 3, 0.99);   // WORK!!!
	
	//MATmtxdisp(funda);

	// find homography transformation
	Mat H_RS = findHomography(matched_pnts_obj, matched_pnts_img, mask, CV_FM_RANSAC, 5);
	// compute transformation matrix    
	//Mat H = getPerspectiveTransform(matched_pnts_obj, matched_pnts_img);
	
	//MatInfo(matched_pnts_obj);
	//MatInfo(matched_pnts_img);
	MATmtxdisp(H_RS);
	//MATmtxdisp(H);
	MATmtxdisp(mask);
	MatInfo(mask);


	// enhanced good match
	vector< DMatch > enh_good_matches;

	for( int i = 0; i < good_matches.size(); i++ )
	{
		if( mask.at<uchar>(i) != 0 )
		{ 
			enh_good_matches.push_back(good_matches[i]);
			cout << i<< endl;
		}
	}

	cout << "Num of Enhanced Match = " << enh_good_matches.size() << endl;
	// output to text
	//LoggerIn.open("log1.txt", fstream::in | fstream::app);
	//LoggerIn << enh_good_matches.size() << endl;
	//LoggerIn.close();
	// eos

	// show enhanced good match
	Mat enh_outimg;
	drawMatches(img, keypoints_obj, img1, keypoints_img, enh_good_matches, enh_outimg);
	imshow("Enhanced Match Result", enh_outimg);

	waitKey(1);

	return 1;
}


//int callSIFTMatch(const Mat Img, const Mat Obj)
int callSIFTMatch(Mat frame, Mat descriptors_ref, vector<KeyPoint> keypoints_ref, Mat ref_img)   // BUG somewhere in here!!!! -> DONE
{
	const int sample_sz = 400;

	Mat img;	// img  : reference image
	Mat img1;   // img1 : current image

	ref_img.copyTo(img);    // ref image
	frame.copyTo(img1);  // current image

	char *org_wind = "the box";
	char *scene_wind = "box in scene";

	
	SIFT SIFTdect_img = SIFT(sample_sz, 3, 0.05, 10, 1.6);

	/*vector<KeyPoint> keypoints_obj, keypoints_img;
	Mat descriptors_obj, descriptors_img; */
	vector<KeyPoint> keypoints_img;
	Mat descriptors_img; 

	
	//SIFTdect_obj(img, noArray(), keypoints_obj, descriptors_obj, false);  // noArray(): using no mask
	SIFTdect_img(img1, noArray(), keypoints_img, descriptors_img, false);    // the order of object & image!!

	//cout << "object =" << endl;
	//MatInfo(img);
	//cout << "scene =" << endl;
	//MatInfo(img1);
	//MatInfo(descriptors_obj);
	//getchar();
	//MATmtxdisp(descriptors_obj);
	//getchar();
	
	//MatInfo(descriptors_img);
	//getchar();
	//MATmtxdisp(descriptors_img);
	//getchar();
	

	Mat pnts_obj = Mat::zeros(1, keypoints_ref.size(), CV_32FC2);
	Mat pnts_img = Mat::zeros(1, keypoints_img.size(), CV_32FC2);
	
	for (int i = 0; i < keypoints_ref.size() ; i++)
	{
		//printf("(%0.2f, %0.2f)\n", keypoints_obj[i].pt.x, keypoints_obj[i].pt.y);
		pnts_obj.at<Vec2f>(i)[0] = keypoints_ref[i].pt.x;
		pnts_obj.at<Vec2f>(i)[1] = keypoints_ref[i].pt.y;
	}
	
	for (int i = 0; i < keypoints_img.size() ; i++)
	{
		//printf("(%0.2f, %0.2f)\n", keypoints_img[i].pt.x, keypoints_img[i].pt.y);
		pnts_img.at<Vec2f>(i)[0] = keypoints_img[i].pt.x;
		pnts_img.at<Vec2f>(i)[1] = keypoints_img[i].pt.y;
	}

	// no need visualization
	//drawCircles(img, pnts_obj);
	//drawCircles(img1, pnts_img);


	// Match keypoints with Brute Force Matcher
	BFMatcher kpMatcher(NORM_L2, false);

	vector<DMatch> matches;

	Mat mask = Mat::zeros(descriptors_ref.rows, descriptors_img.rows, CV_8UC1);
	//mask.at<float>(keypoints_obj[0].pt.y, keypoints_obj[0].pt.x) = descriptors_obj.at<float>(keypoints_obj[0].pt.y, keypoints_obj[0].pt.x);

	if (sample_sz <= descriptors_ref.rows) 
	{
		Mat maskinit = Mat::ones(sample_sz, descriptors_img.rows, CV_8UC1);
		maskinit.copyTo(mask(Range(0, sample_sz), Range(0, descriptors_img.rows)));
	} else
	{
		Mat maskinit = Mat::ones(descriptors_ref.rows, descriptors_img.rows, CV_8UC1);
		maskinit.copyTo(mask(Range(0, descriptors_ref.rows), Range(0, descriptors_img.rows)));
	}

	kpMatcher.match(descriptors_ref, descriptors_img, matches, mask);   // no use mask -> skip the arg

		
	cout << "Number of matches = " << matches.size() << endl;

	//imshow(org_wind, img);
	//imshow(scene_wind, img1);
	
	
	// Using FUNDAMENTAL matrix
	// read matches
	/*
	DMatch match = matches[0];
	cout << match.queryIdx << " matches with " << match.imgIdx << " img idx " << match.imgIdx << endl;
	
	for (int i = 0; i < matches.size(); i++)
	{
		match = matches[i];
		cout << match.queryIdx <<  " -> " << match.trainIdx << endl;
	}
	*/
	
	double max_dist = 0; double min_dist = 500;

	//-- Quick calculation of max and min distances between keypoints
	for( int i = 0; i < matches.size(); i++ )
	{	DMatch match = matches[i];
		float dist = match.distance;
		if( dist < min_dist ) min_dist = dist;
		if( dist > max_dist ) max_dist = dist;
	}
	
	cout << "max_dist = " << max_dist << endl;
	cout << "min_dist = " << min_dist << endl;

	// find the good matches
	
	//-- Draw only "good" matches (i.e. whose distance is less than 3*min_dist )
	std::vector< DMatch > good_matches;

	for( int i = 0; i < matches.size(); i++ )
	{
		//if( matches[i].distance < 30*min_dist )   // change 3/4/5/6
		{ good_matches.push_back( matches[i]); }    // get all matches
	}
	

	// Show results of matching (good matches)
	Mat outimg;
	//drawMatches(img, keypoints_obj, img1, keypoints_img, good_matches, outimg);
	drawMatches(img, keypoints_ref, img1, keypoints_img, good_matches, outimg, Scalar(255,0,0));  // updated 14/05/13
	//imshow("Match result", outimg);


	// call XtractMachingPoints
	Mat matched_pnts_obj, matched_pnts_img;
	XtractMatchingPoints(keypoints_ref, keypoints_img, good_matches, matched_pnts_obj, matched_pnts_img);
	
	//Mat funda = findFundamentalMat(matched_pnts_obj, matched_pnts_img, noArray(), CV_FM_8POINT);  -> can't work
	//Mat funda = findFundamentalMat(matched_pnts_obj, matched_pnts_img, noArray, FM_RANSAC, 3, 0.8); -> can't work
	//Mat funda =	findFundamentalMat(matched_pnts_obj, matched_pnts_img, FM_RANSAC, 3, 0.99);   // WORK!!!
	
	//MATmtxdisp(funda);

	// find homography transformation
	Mat H_RS = findHomography(matched_pnts_obj, matched_pnts_img, mask, CV_FM_RANSAC, 5);
	// compute transformation matrix    
	//Mat H = getPerspectiveTransform(matched_pnts_obj, matched_pnts_img);
	
	//MatInfo(matched_pnts_obj);
	//MatInfo(matched_pnts_img);
	//MATmtxdisp(H_RS);
	//MATmtxdisp(H);
	//MATmtxdisp(mask);
	//MatInfo(mask);


	// enhanced good match
	vector< DMatch > enh_good_matches;

	for( int i = 0; i < good_matches.size(); i++ )
	{
		if( mask.at<uchar>(i) != 0 )
		{ 
			enh_good_matches.push_back(good_matches[i]);
			//cout << i<< endl;
		}
	}

	cout << "Num of Enhanced Match = " << enh_good_matches.size() << endl;
	// output to text
	/*LoggerIn.open("log1.txt", fstream::in | fstream::app);
	LoggerIn << enh_good_matches.size() << endl;
	LoggerIn.close();*/
	// eos
	int ret_matches = enh_good_matches.size();

	// show enhanced good match
	Mat enh_outimg;
	drawMatches(img, keypoints_ref, img1, keypoints_img, enh_good_matches, enh_outimg, Scalar(255,0,0));  // updated 14/05/13
	imshow("Enhanced Match Result", enh_outimg);

	//waitKey(0);
	waitKey(1);

	return ret_matches;
}

// Date Created: 17/04/13
// Descriptions: with returning correspondence
int callSIFTMatch(Mat test_img, Mat descriptors_obj, vector<KeyPoint> keypoints_obj, Mat obj_img, 
				  vector<KeyPoint> keypoints_img_ret, vector<DMatch> enh_good_matches_ret)   // BUG somewhere in here!!!! -> DONE
{
	const int sample_sz = 400;

	//char *imgfile0 = "box.png";
	//char *imgfile0 = "Picture 98.jpg";
	//char *imgfile0 = "./Debug/testing_images/Picture 111_seg.jpg";
	//char *imgfile0 = argv[1];
//	char *imgfile1 = "box_in_scene.png";
	//char *imgfile1 = "Picture 99.jpg";
	//char *imgfile1 = "./Debug/Picture 85_seg.jpg";
	//char *imgfile1 = argv[2];

	//Mat img = imread(imgfile0, 0);
	//Mat img1 = imread(imgfile1, 0);
	Mat img;
	Mat img1;

	obj_img.copyTo(img);
	test_img.copyTo(img1);

	char *org_wind = "the box";
	char *scene_wind = "box in scene";

	
	//SIFT SIFTdect_obj = SIFT(sample_sz, 3, 0.04, 10, 1.6);
	//SIFT SIFTdect_obj = SIFT(sample_sz, 3, 0.05, 10, 1.6);
	
	//SIFT SIFTdect_img = SIFT(sample_sz, 3, 0.04, 10, 1.6);
	SIFT SIFTdect_img = SIFT(sample_sz, 3, 0.05, 10, 1.6);

	/*vector<KeyPoint> keypoints_obj, keypoints_img;
	Mat descriptors_obj, descriptors_img; */
	vector<KeyPoint> keypoints_img;
	Mat descriptors_img; 

	
	//SIFTdect_obj(img, noArray(), keypoints_obj, descriptors_obj, false);  // noArray(): using no mask
	SIFTdect_img(img1, noArray(), keypoints_img, descriptors_img, false);    // the order of object & image!!

	//cout << "object =" << endl;
	//MatInfo(img);
	//cout << "scene =" << endl;
	//MatInfo(img1);
	//MatInfo(descriptors_obj);
	//getchar();
	//MATmtxdisp(descriptors_obj);
	//getchar();
	
	//MatInfo(descriptors_img);
	//getchar();
	//MATmtxdisp(descriptors_img);
	//getchar();
	

	Mat pnts_obj = Mat::zeros(1, keypoints_obj.size(), CV_32FC2);
	Mat pnts_img = Mat::zeros(1, keypoints_img.size(), CV_32FC2);
	
	for (int i = 0; i < keypoints_obj.size() ; i++)
	{
		//printf("(%0.2f, %0.2f)\n", keypoints_obj[i].pt.x, keypoints_obj[i].pt.y);
		pnts_obj.at<Vec2f>(i)[0] = keypoints_obj[i].pt.x;
		pnts_obj.at<Vec2f>(i)[1] = keypoints_obj[i].pt.y;
	}
	
	for (int i = 0; i < keypoints_img.size() ; i++)
	{
		//printf("(%0.2f, %0.2f)\n", keypoints_img[i].pt.x, keypoints_img[i].pt.y);
		pnts_img.at<Vec2f>(i)[0] = keypoints_img[i].pt.x;
		pnts_img.at<Vec2f>(i)[1] = keypoints_img[i].pt.y;
	}

	// no need visualization
	//drawCircles(img, pnts_obj);
	//drawCircles(img1, pnts_img);


	// Match keypoints with Brute Force Matcher
	BFMatcher kpMatcher(NORM_L2, false);

	vector<DMatch> matches;

	Mat mask = Mat::zeros(descriptors_obj.rows, descriptors_img.rows, CV_8UC1);
	//mask.at<float>(keypoints_obj[0].pt.y, keypoints_obj[0].pt.x) = descriptors_obj.at<float>(keypoints_obj[0].pt.y, keypoints_obj[0].pt.x);

	if (sample_sz <= descriptors_obj.rows) 
	{
		Mat maskinit = Mat::ones(sample_sz, descriptors_img.rows, CV_8UC1);
		maskinit.copyTo(mask(Range(0, sample_sz), Range(0, descriptors_img.rows)));
	} else
	{
		Mat maskinit = Mat::ones(descriptors_obj.rows, descriptors_img.rows, CV_8UC1);
		maskinit.copyTo(mask(Range(0, descriptors_obj.rows), Range(0, descriptors_img.rows)));
	}

	kpMatcher.match(descriptors_obj, descriptors_img, matches, mask);   // no use mask -> skip the arg

		
	cout << "Number of matches = " << matches.size() << endl;

	//imshow(org_wind, img);
	//imshow(scene_wind, img1);
	
	
	// Using FUNDAMENTAL matrix
	// read matches
	/*
	DMatch match = matches[0];
	cout << match.queryIdx << " matches with " << match.imgIdx << " img idx " << match.imgIdx << endl;
	
	for (int i = 0; i < matches.size(); i++)
	{
		match = matches[i];
		cout << match.queryIdx <<  " -> " << match.trainIdx << endl;
	}
	*/
	
	double max_dist = 0; double min_dist = 500;

	//-- Quick calculation of max and min distances between keypoints
	for( int i = 0; i < matches.size(); i++ )
	{	DMatch match = matches[i];
		float dist = match.distance;
		if( dist < min_dist ) min_dist = dist;
		if( dist > max_dist ) max_dist = dist;
	}
	
	cout << "max_dist = " << max_dist << endl;
	cout << "min_dist = " << min_dist << endl;

	// find the good matches
	
	//-- Draw only "good" matches (i.e. whose distance is less than 3*min_dist )
	std::vector< DMatch > good_matches;

	for( int i = 0; i < matches.size(); i++ )
	{
		//if( matches[i].distance < 30*min_dist )   // change 3/4/5/6
		{ good_matches.push_back( matches[i]); }    // get all matches
	}
	

	// Show results of matching (good matches)
	Mat outimg;
	drawMatches(img, keypoints_obj, img1, keypoints_img, good_matches, outimg, Scalar(255,0,0));  // updated 14/05/13
	//imshow("Match result", outimg);


	// call XtractMachingPoints
	Mat matched_pnts_obj, matched_pnts_img;
	XtractMatchingPoints(keypoints_obj, keypoints_img, good_matches, matched_pnts_obj, matched_pnts_img);
	
	//Mat funda = findFundamentalMat(matched_pnts_obj, matched_pnts_img, noArray(), CV_FM_8POINT);  -> can't work
	//Mat funda = findFundamentalMat(matched_pnts_obj, matched_pnts_img, noArray, FM_RANSAC, 3, 0.8); -> can't work
	//Mat funda =	findFundamentalMat(matched_pnts_obj, matched_pnts_img, FM_RANSAC, 3, 0.99);   // WORK!!!
	
	//MATmtxdisp(funda);

	// find homography transformation
	Mat H_RS = findHomography(matched_pnts_obj, matched_pnts_img, mask, CV_FM_RANSAC, 5);
	// compute transformation matrix    
	//Mat H = getPerspectiveTransform(matched_pnts_obj, matched_pnts_img);
	
	//MatInfo(matched_pnts_obj);
	//MatInfo(matched_pnts_img);
	//MATmtxdisp(H_RS);
	//MATmtxdisp(H);
	//MATmtxdisp(mask);
	//MatInfo(mask);


	// enhanced good match
	vector< DMatch > enh_good_matches;

	for( int i = 0; i < good_matches.size(); i++ )
	{
		if( mask.at<uchar>(i) != 0 )
		{ 
			enh_good_matches.push_back(good_matches[i]);
			//cout << i<< endl;
		}
	}

	cout << "Num of Enhanced Match = " << enh_good_matches.size() << endl;
	// output to text
	/*LoggerIn.open("log1.txt", fstream::in | fstream::app);
	LoggerIn << enh_good_matches.size() << endl;
	LoggerIn.close();*/
	// eos
	int ret_matches = enh_good_matches.size();

	// show enhanced good match
	Mat enh_outimg;
	drawMatches(img, keypoints_obj, img1, keypoints_img, enh_good_matches, enh_outimg, Scalar(255,0,0));  // updated 14/05/13
	imshow("Enhanced Match Result", enh_outimg);

	//waitKey(0);
	waitKey(1);

	// return correspondences
	keypoints_img_ret = keypoints_img;  // RISK!!!
	enh_good_matches_ret = enh_good_matches;  // RISK!!

	return ret_matches;
}



// Date Created: 19/04/13
// Descriptions: with returning correspondence (both descriptor & keypoints)
int callSIFTMatch(Mat frame, Mat descriptors_ref, vector<KeyPoint> keypoints_ref, Mat ref_img, 
				  vector<KeyPoint>& keypoints_img_ret, Mat& descriptors_img_ret,vector<DMatch>& enh_good_matches_ret)   // BUG somewhere in here!!!! -> DONE
{
	const int sample_sz = 400;

	Mat img;
	Mat img1;

	ref_img.copyTo(img);
	frame.copyTo(img1);

	char *org_wind = "the box";
	char *scene_wind = "box in scene";

	
	//SIFT SIFTdect_obj = SIFT(sample_sz, 3, 0.04, 10, 1.6);
	//SIFT SIFTdect_obj = SIFT(sample_sz, 3, 0.05, 10, 1.6);
	
	//SIFT SIFTdect_img = SIFT(sample_sz, 3, 0.04, 10, 1.6);
	SIFT SIFTdect_img = SIFT(sample_sz, 3, 0.05, 10, 1.6);

	/*vector<KeyPoint> keypoints_obj, keypoints_img;
	Mat descriptors_obj, descriptors_img; */
	vector<KeyPoint> keypoints_img;
	Mat descriptors_img; 

	
	//SIFTdect_obj(img, noArray(), keypoints_obj, descriptors_obj, false);  // noArray(): using no mask
	SIFTdect_img(img1, noArray(), keypoints_img, descriptors_img, false);    // the order of object & image!!

	//cout << "object =" << endl;
	//MatInfo(img);
	//cout << "scene =" << endl;
	//MatInfo(img1);
	//MatInfo(descriptors_obj);
	//getchar();
	//MATmtxdisp(descriptors_obj);
	//getchar();
	
	//MatInfo(descriptors_img);
	//getchar();
	//MATmtxdisp(descriptors_img);
	//getchar();
	

	Mat pnts_obj = Mat::zeros(1, keypoints_ref.size(), CV_32FC2);
	Mat pnts_img = Mat::zeros(1, keypoints_img.size(), CV_32FC2);
	
	for (int i = 0; i < keypoints_ref.size() ; i++)
	{
		//printf("(%0.2f, %0.2f)\n", keypoints_obj[i].pt.x, keypoints_obj[i].pt.y);
		pnts_obj.at<Vec2f>(i)[0] = keypoints_ref[i].pt.x;
		pnts_obj.at<Vec2f>(i)[1] = keypoints_ref[i].pt.y;
	}
	
	for (int i = 0; i < keypoints_img.size() ; i++)
	{
		//printf("(%0.2f, %0.2f)\n", keypoints_img[i].pt.x, keypoints_img[i].pt.y);
		pnts_img.at<Vec2f>(i)[0] = keypoints_img[i].pt.x;
		pnts_img.at<Vec2f>(i)[1] = keypoints_img[i].pt.y;
	}

	// no need visualization
	//drawCircles(img, pnts_obj);
	//drawCircles(img1, pnts_img);


	// Match keypoints with Brute Force Matcher
	BFMatcher kpMatcher(NORM_L2, false);

	vector<DMatch> matches;

	Mat mask = Mat::zeros(descriptors_ref.rows, descriptors_img.rows, CV_8UC1);
	//mask.at<float>(keypoints_obj[0].pt.y, keypoints_obj[0].pt.x) = descriptors_obj.at<float>(keypoints_obj[0].pt.y, keypoints_obj[0].pt.x);

	if (sample_sz <= descriptors_ref.rows) 
	{
		Mat maskinit = Mat::ones(sample_sz, descriptors_img.rows, CV_8UC1);
		maskinit.copyTo(mask(Range(0, sample_sz), Range(0, descriptors_img.rows)));
	} else
	{
		Mat maskinit = Mat::ones(descriptors_ref.rows, descriptors_img.rows, CV_8UC1);
		maskinit.copyTo(mask(Range(0, descriptors_ref.rows), Range(0, descriptors_img.rows)));
	}

	kpMatcher.match(descriptors_ref, descriptors_img, matches, mask);   // no use mask -> skip the arg

		
	cout << "Number of matches = " << matches.size() << endl;

	//imshow(org_wind, img);
	//imshow(scene_wind, img1);
	
	
	// Using FUNDAMENTAL matrix
	// read matches
	/*
	DMatch match = matches[0];
	cout << match.queryIdx << " matches with " << match.imgIdx << " img idx " << match.imgIdx << endl;
	
	for (int i = 0; i < matches.size(); i++)
	{
		match = matches[i];
		cout << match.queryIdx <<  " -> " << match.trainIdx << endl;
	}
	*/
	
	double max_dist = 0; double min_dist = 500;

	//-- Quick calculation of max and min distances between keypoints
	for( int i = 0; i < matches.size(); i++ )
	{	DMatch match = matches[i];
		float dist = match.distance;
		if( dist < min_dist ) min_dist = dist;
		if( dist > max_dist ) max_dist = dist;
	}
	
	cout << "max_dist = " << max_dist << endl;
	cout << "min_dist = " << min_dist << endl;

	// find the good matches
	
	//-- Draw only "good" matches (i.e. whose distance is less than 3*min_dist )
	std::vector< DMatch > good_matches;

	for( int i = 0; i < matches.size(); i++ )
	{
		//if( matches[i].distance < 30*min_dist )   // change 3/4/5/6
		{ good_matches.push_back( matches[i]); }    // get all matches
	}
	

	// Show results of matching (good matches)
	Mat outimg;
	drawMatches(img, keypoints_ref, img1, keypoints_img, good_matches, outimg, Scalar(255,0,0));  // updated 14/05/13
	//imshow("Match result", outimg);


	// call XtractMachingPoints
	Mat matched_pnts_obj, matched_pnts_img;
	XtractMatchingPoints(keypoints_ref, keypoints_img, good_matches, matched_pnts_obj, matched_pnts_img);
	
	//Mat funda = findFundamentalMat(matched_pnts_obj, matched_pnts_img, noArray(), CV_FM_8POINT);  -> can't work
	//Mat funda = findFundamentalMat(matched_pnts_obj, matched_pnts_img, noArray, FM_RANSAC, 3, 0.8); -> can't work
	//Mat funda =	findFundamentalMat(matched_pnts_obj, matched_pnts_img, FM_RANSAC, 3, 0.99);   // WORK!!!
	
	//MATmtxdisp(funda);

	// find homography transformation
	Mat H_RS = findHomography(matched_pnts_obj, matched_pnts_img, mask, CV_FM_RANSAC, 3); // 1,2,3
	// compute transformation matrix    
	//Mat H = getPerspectiveTransform(matched_pnts_obj, matched_pnts_img);
	
	//MatInfo(matched_pnts_obj);
	//MatInfo(matched_pnts_img);
	//MATmtxdisp(H_RS);
	//MATmtxdisp(H);
	//MATmtxdisp(mask);
	//MatInfo(mask);


	// enhanced good match
	vector< DMatch > enh_good_matches;

	for( int i = 0; i < good_matches.size(); i++ )
	{
		if( mask.at<uchar>(i) != 0 )
		{ 
			enh_good_matches.push_back(good_matches[i]);
			//cout << i<< endl;
		}
	}

	cout << "Num of Enhanced Match = " << enh_good_matches.size() << endl;
	// output to text
	/*LoggerIn.open("log1.txt", fstream::in | fstream::app);
	LoggerIn << enh_good_matches.size() << endl;
	LoggerIn.close();*/
	// eos
	int ret_matches = enh_good_matches.size();

	// show enhanced good match
	Mat enh_outimg;
	// draw all matches
//	drawMatches(img, keypoints_obj, img1, keypoints_img, enh_good_matches, enh_outimg, Scalar(255,0,0));
	
	// only draw selected matches
	vector<DMatch>::const_iterator first = enh_good_matches.begin();
	vector<DMatch>::const_iterator last = enh_good_matches.begin() + enh_good_matches.size(); ///2;
	vector<DMatch> sub_matches(first, last);

	drawMatches(img, keypoints_ref, img1, keypoints_img, sub_matches, enh_outimg, Scalar(255,0,0));

	// check keypoint.pt.x = col?? pt.y = row?? => CORRECT!!!
	//circle(enh_outimg, Point(keypoints_obj[sub_matches[2].queryIdx].pt.x, keypoints_obj[sub_matches[2].queryIdx].pt.y), 30, Scalar(0,255,0), 3);
	
	// draw test point
	//int col = 250, row = 100;
	//circle(enh_outimg, Point(col, row), 30, Scalar(0,255,0), 3);

	imshow("Enhanced Match Result", enh_outimg);

	//waitKey(0);
	waitKey(1);

	// return correspondences
	keypoints_img_ret = keypoints_img;  // RISK!!!
	enh_good_matches_ret = enh_good_matches;  // RISK!!
	descriptors_img.copyTo(descriptors_img_ret);

	return ret_matches;
}



// Date Created: 05/04/13
// Descriptions:
// Input:  img, db_img, db_keypoints, db_descriptors
// Output: obj_id;
int ObjectDetect(const cv::Mat test_img, 
				 std::vector<std::vector<std::vector<cv::KeyPoint>>> db_keypoints, 
				 std::vector<std::vector<cv::Mat>> db_descriptors, 
				 std::vector<std::vector<cv::Mat>> hier_db_img)
{

	int id_votes[20];
	int vote, max_vote;
	
	// Iterate through objects - for each object get 2 samples
	for (int i = 0; i < hier_db_img.size(); i++)
	{
		max_vote = 0;

		//for (int j = 0; j < 2; j++)
		for (int j = 0; j < hier_db_img[i].size(); j++)
		{
			// Call Sift Matching with Enhance to get No. Match
			vote = callSIFTMatch(test_img, db_descriptors[i][j], db_keypoints[i][j], hier_db_img[i][j]);

			cout << "i="<< i << "j=" << j << "Vote = " << vote << endl;

			getchar();

			if (vote > max_vote)
			{
				max_vote = vote;
			}
		}

		id_votes[i] = max_vote;
	}

	// return max object vote
	int ret_id=0;
	max_vote = 0;

	for (int i=0; i<hier_db_img.size(); i++)
		if (id_votes[i] > max_vote)
		{
			max_vote = id_votes[i];
			ret_id = i+1;
		}

	cout << "Max Vote = " << max_vote << endl;
	int det_thres = 20;
	if (max_vote < det_thres)
	{
		ret_id = 0;
	}

	return ret_id;
}